//------------------------------------------------------------------------------
//  <copyright file="SimpleMockFRIRemote.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "stdafx.h"
#include "SimpleMockFRIRemote.h"
#include <sstream>

// precision of inputs in test is low in some cases -- this level of error would be expeted to be inconsequential
// (essentially in the noise) on the LBR hardware.
#define KRL_TO_REAL_ERROR_EPSILON  0.001

/// <summary> Initializes a new instance of the SimpleMockFRIRemote controller class </summary>
SimpleMockFRIRemote::SimpleMockFRIRemote(int port, char * hintToRemoteHost)
{
	this->doDataExchangeCount = 0; 
	this->doReceiveDataCount = 0;
	this->doSendDataCount = 0;
	this->doPositionControlCount = 0;
	this->doJntImpedanceControlCount = 0;
	this->doCartesianImpedanceControlCount = 0;
	this->getStateCount = 0;
	this->getQualityCount = 0;
	this->getMsrMsrJntPositionCount = 0;
	this->getMsrCmdCartPositionCount = 0;
	this->getMsrCartPositionCount = 0;
	this->getMsrBufCount = 0;
}

/// <summary> Execute a position control command. </summary>
/// <param name="newJntPosition"> Target joint position. </param>
/// <param name="flagDataExchange"> 
/// Indicates whether or not to automatically exchange data with the FRI client before returning.
/// </param>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange)
{
	++this->doPositionControlCount;

	for (int i = 0; i<LBR_MNJ; ++i) 
	{
		if (newJntPosition[i] != this->expectedJointPosition[i])
		{
			throw "Joint positions passed to doPositionControl do not match exepected positions.";
		}
	}

	this->jointPositionCommandReceived = true;

	return 1;
}

/// <summary> Execute a cartesion impedance command </summary>
/// <param name="newCartStiff"> Cartesian stiffness. Null value ignored. </param>
/// <param name="newCartDam"> Cartesian damping.  Null value ignored. </param>
/// <param name="newAddTcpFT> Additional force and torque on the TCP. </param>
/// <param name="newJntNullspace"> Joint nullspace. </param>
/// <param name="flagDataExchange"> 
/// Indicates whether or not to automatically exchange data with the FRI client before returning.
/// </param>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], 
													 const float newCartStiff[FRI_CART_VEC], 
													 const float newCartDamp[FRI_CART_VEC], 
													 const float newAddTcpFT[FRI_CART_VEC],
													 const float newJntNullspace[LBR_MNJ],
													 bool flagDataExchange)
{
	++this->doCartesianImpedanceControlCount;

	for (int i = 0; i<FRI_CART_FRM_DIM; ++i)
	{
		if (newCartPosition[i] != this->expectedCartesianPosition[i])
		{
			throw "Cartesian positions passed to doCartesianImpedanceControl do not match exepected positions.";
		}
	}

	this->cartesianPositionCommandReceived = true;

	return 1;
}

/// <summary> Get the current measured joint position. </summary>
/// <returns> The current measured joint position. </returns>
float* SimpleMockFRIRemote::getMsrMsrJntPosition()
{
	++this->getMsrMsrJntPositionCount;
	return this->FRIMsrData.data.msrJntPos;
}

/// <summary> Get the current measured Cartesian position of TCP. </summary>
/// <returns> The current measured Cartesian position of TCP. </returns>
float *SimpleMockFRIRemote::getMsrCmdCartPosition()
{
	++this->getMsrCmdCartPositionCount;
	return this->FRIMsrData.data.cmdCartPos;
}

/// <summary> Get the data stored in the msr buffer. </summary>
/// <returns> The data stored in the msr buffer. </returns>
tFriMsrData & SimpleMockFRIRemote::getMsrBuf()
{
	++this->getMsrBufCount;
	return this->FRIMsrData;
}

/// <summary> The quality that will be returned by <c>getQuality</c> </summary>
FRI_QUALITY SimpleMockFRIRemote::getQuality()
{
	++this->getQualityCount;
	return this->FRIQuality;
}

/// <summary> Get the current measured Cartesian position of TCP. </summary>
/// <returns> The current measured Cartesian position of TCP. </returns>
float* SimpleMockFRIRemote::getMsrCartPosition()
{
	++this->getMsrCartPositionCount;
	return this->FRIMsrData.data.msrCartPos;
}

/// <summary> Excange data with FRI client.  Blocks on next packet from FRI client. </summary>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doDataExchange() 
{ 
	++this->doDataExchangeCount;
	return 1; 
}

/// <summary> Receive data from FRI client.  Blocks on next packet from FRI client. </summary>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doReceiveData() 
{ 
	++this->doReceiveDataCount;
	return 1; 
}

/// <summary> Send data to the FRI client.  </summary>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doSendData() 
{ 
	++this->doSendDataCount;
	return 1; 
}

/// <summary> Executes a joint impdance control command. </summary>
/// <param name="newJntPosition"> Joint positions.  Null value ignored. </param>
/// <param name="newJntStiff"> Joint stiffness.  Null value ignored. </param>
/// <param name="newJntDamp"> Joint damping.  Null value ignored.   </param>
/// <param name="newJntAddTorque"> Additional torque.  Null values ignored. </param>
/// <param name="flagDataExchange"> 
/// Indicates whether or not to automatically exchange data with the FRI client before returning.
/// </param>
/// <returns> 0 on success.  -1 on failure. </returns>
int SimpleMockFRIRemote::doJntImpedanceControl(
	const float newJntPosition[LBR_MNJ], 
	const float newJntStiff[LBR_MNJ],
	const float newJntDamp[LBR_MNJ],
	const float newJntAddTorque[LBR_MNJ],
	bool flagDataExchange) 
{ 
	++this->doJntImpedanceControlCount;
	return 1; 
}

/// <summary> Get the state of the FRI connection. </summary>
/// <returns> State of the FRI connection. </returns>
FRI_STATE SimpleMockFRIRemote::getState() 
{ 
	++this->getStateCount;
	return FRI_STATE::FRI_STATE_CMD; 
}

/// <summary> Set a real value to KRL. Corresponds to KRL variable $FRI_FRM_REA </summary>
/// <param name=index> Index of the real value to set. </param>
/// <param name=val> Value to set. </param>
void SimpleMockFRIRemote::setToKRLReal(int index, float val)
{
	std::ostringstream errorString;

	if (index >= FRI_USER_SIZE)
	{
		errorString << "Invalid ToKRLReal index value: " << index;
		throw  errorString.str();
	}
	
	if (fabs(this->expectedToKRLReal[index] - val) > KRL_TO_REAL_ERROR_EPSILON)
	{
		errorString << "Unexpected value: " << val << " for ToKRLReal index: " << index;
		throw errorString.str();
	}
}

/// <summary> Set a integer value to KRL. Corresponds to KRL variable $FRI_FRM_INT </summary>
/// <param name=index> Index of the integer value to set. </param>
/// <param name=val> Value to set. </param>
void SimpleMockFRIRemote::setToKRLInt(int index, int val)
{
	std::ostringstream errorString;

	if (index >= FRI_USER_SIZE)
	{
		errorString << "Invalid ToKRLInt index value: " << index;
		throw  errorString.str();
	}
	
	if (this->expectedToKRLInt[index] != val)
	{
		errorString << "Unexpected value: " << val << " for ToKRLInt index: " << index;
		throw errorString.str();
	}
}
